
import image,sensor,lcd,time,math
from machine import Timer
from machine import UART #串口库函数
from fpioa_manager import fm # GPIO重定向函数
from board import board_info
from maix import GPIO

'''
矩形元组四变量依次的意义。以下方位均以当可以正向看到板子上的丝印时为准
1.右边缘从右向左数有几个像素。大于320就出界了，虽然函数不会报错，但不会有任何影响。下面的三个应该也是一样的
2.下边缘从下往上数有几个像素
3.横向宽度
4.纵向宽度
'''

'''坐标值以像素为单位，240像素对应50厘米。以左上角为原点，向右、向下为x、y的正方向。所以使用的时候务必把摄像范围和地图二者的左上角对齐'''

'''thresholds 必须是元组列表。 [(lo, hi), (lo, hi), …, (lo, hi)] 定义你想追踪的颜色范围。 对于灰度图像，每个元组需要包含两个值 - 最小灰度值和最大灰度值。
仅考虑落在这些阈值之间的像素区域。 对于RGB565图像，每个元组需要有六个值(l_lo，l_hi，a_lo，a_hi，b_lo，b_hi) - 分别是LAB L，A和B通道的最小值和最大值。 为方便
使用，此功能将自动修复交换的最小值和最大值。 此外，如果元组大于六个值，则忽略其余值。相反，如果元组太短，则假定其余阈值处于最大范围。'''
red_threshold = [(100, 60, 47, -2, -11, 10)]# 追踪红色斑点

green_threshold = [(93, 30, -60, 2, 10, 68)]# 追踪绿色斑点。其中该阈值是在已经进行了一次红色查询之后的，相比较初始图像去查询绿色阈值范围并不一样

lcd.init()
sensor.reset(dual_buff=False)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)

spot_location = [0, 0, 0, 0] # 记录红色斑点水平、竖直位置；绿色斑点水平、竖直位置的列表。之所以是列表是因为它的值是还要被修改的，后面要用时会转化为元组。
line_orientation = [0, 0] # 记录目标位置水平、竖直坐标。同上

fm.register(18, fm.fpioa.UART1_TX, force=True) # 设置18、19引脚为与控制红色斑点的stm32进行通信的引脚
fm.register(19, fm.fpioa.UART1_RX, force=True)

fm.register(10, fm.fpioa.UART2_TX, force=True) # 设置10、11引脚为与控制绿色斑点的stm32进行通信的引脚
fm.register(11, fm.fpioa.UART2_RX, force=True)

uart_A = UART(UART.UART1, 115200, 8, 0, 1, timeout=1000, read_buf_len=4096)
uart_B = UART(UART.UART2, 115200, 8, 0, 1, timeout=1000, read_buf_len=4096)


# 串口数据包格式：FF 目标x 目标y FE。通过形参uart指定需要向哪个串口发送数据，从而指定接收方
def sending_data(x, y, uart):
    FH = bytearray([0xFF, x, y, 0xFE])
    uart.write(FH) # 串口发送数据，其中的十六进制数字是帧头、帧尾

def find_red_spot(img):
    blob = img.find_blobs(red_threshold, pixels_threshold=100, area_threshold=300, merge=True)
    if blob: # 有时可能会一个色块都找不到，这种时候该列表就是空的，需要加以判断。
        largest_blob = max(blob, key=lambda b: b.pixels())# 找出最大的色块
        if largest_blob.code() == 1:
            spot_location[0] = largest_blob.cx()
            spot_location[1] = largest_blob.cy()
            img.draw_rectangle(largest_blob.rect())
            img.draw_cross(spot_location[0], spot_location[1])
            img.draw_keypoints([(spot_location[0], spot_location[1],
                                 int(math.degrees(largest_blob.rotation())))], size=20)
            return 0
    return 1

def find_green_spot(img):
    blob = img.find_blobs(green_threshold, pixels_threshold=100, area_threshold=300, merge=True)
    if blob: # 有时可能会一个色块都找不到，这种时候该列表就是空的，需要加以判断。
        largest_blob = max(blob, key=lambda b: b.pixels())# 找出最大的色块
        if largest_blob.code() == 1:
            rectangle = largest_blob.rect()
            spot_location[2] = largest_blob.cx() - rectangle[2] // 2
            spot_location[3] = largest_blob.cy() - rectangle[3] // 2
            img.draw_rectangle(rectangle)
            img.draw_cross(largest_blob.cx(), largest_blob.cy())
            img.draw_keypoints([(largest_blob.cx(), largest_blob.cy(),
                                 int(math.degrees(largest_blob.rotation())))], size=20)

interpolation_num = 3  # 每条边插入点的个数
total_num = 4 + interpolation_num * 4  # 共有几个点
target_table = [i for i in range(total_num)]
'''
识别矩形边框，插入中间点
'''
def find_total_way():
    while True:
        img = sensor.snapshot()
        r = img.find_rects(threshold = 10000)
        if len(r) != 0:
            r_len = len(r)
            for k in range(r_len):
                for m in range(2):
                    img.draw_rectangle(r[k].rect(), color = (255, 0, 0))
                    vertex = [p for p in r[k].corners()]  # 顶点
                    vertex.append(vertex[0])
                    for i in range(4):
                        delta_x = (vertex[i + 1][0] - vertex[i][0]) // (interpolation_num + 1)
                        delta_y = (vertex[i + 1][1] - vertex[i][1]) // (interpolation_num + 1)
                        for j in range(interpolation_num + 1):
                            if j == 0:
                                target_table[i * (interpolation_num + 1) + j] = vertex[i]
                                img.draw_circle(vertex[i][0], vertex[i][1], 5, color = (0, 255, 0))
                            else:
                                target_table[i * (interpolation_num + 1) + j] = (vertex[i][0] + j * delta_x, vertex[i][1] + j * delta_y)
                                img.draw_circle(target_table[i * (interpolation_num + 1) + j][0], target_table[i * (interpolation_num + 1) + j][1], 5, color = (255, 0, 0))
                    lcd.display(img)
                    while True:
                        tmp = uart_A.read()
                        if tmp != None:
                            print(tmp)
                            if tmp == b'333':
                                return None
                            else:
                                break
        else:
            lcd.display(img)

'''
传入当前处于第几条边的第几个点，将该点坐标与偏移量叠加，发送出去。每个点的偏移量不尽相同，因此一旦切换点了之后就应该清零偏移量
'''
def send_next_way(img):
    global if_continue_send
    global now_side
    global now_point
    global delta_x
    global delta_y
    if if_continue_send == True:
        print(target_table[now_side * (interpolation_num + 1) + now_point][0], target_table[now_side * (interpolation_num + 1) + now_point][1])
        sending_data(target_table[now_side * (interpolation_num + 1) + now_point][0] + delta_x, target_table[now_side * (interpolation_num + 1) + now_point][1] + delta_y, uart_A)
        #sending_data(spot_location[0], spot_location[1], uart_B)

        # 进行红色激光斑点查询。有时可能出现找不到光斑的情况，只好认为它就在电工胶上而不明显，无需调整了。此时查询函数返回值应该是1
        ret = find_red_spot(img)

        #if ret or (math.fabs(spot_location[0] - target_table[now_side * (interpolation_num + 1) + now_point][0]) <= 1 and math.fabs(spot_location[1] - target_table[now_side * (interpolation_num + 1) + now_point][1]) <= 1):
        # 误差可接受，或者根本就找不到点，切换至下一点
        if now_point == interpolation_num and now_side != 3:
            now_point = 0
            now_side += 1
        elif now_point != interpolation_num:
            now_point += 1
        #else:  # 误差较大，需要继续调整
            #if spot_location[0] - target_table[now_side * (interpolation_num + 1) + now_point][0] > 0:
                #delta_x += 1
            #else:
                #delta_x -= 1
            #if spot_location[1] - target_table[now_side * (interpolation_num + 1) + now_point][1] > 0:
                #delta_y += 1
            #else:
                #delta_y -= 1
        if_continue_send = False

def Question_1():
    if if_continue_send == True:
        sending_data(60, 60, uart_A)
        sending_data(60, 60, uart_B)
        #print("已成功发送")

def Question_2():
    global moving_state
    global moving_table

    if if_continue_send == True:
        sending_data(moving_table[moving_state][0], moving_table[moving_state][1], uart_A)
        sending_data(moving_table[moving_state][0], moving_table[moving_state][1], uart_B)
        #print("已成功发送")
        #print(moving_table[moving_state][0], moving_table[moving_state][1])
        if moving_state != 5:
            moving_state += 1
        else:
            moving_state = 0
            return

'''
对于每个检查点均会因为各种原因导致摄像头照到的坐标与实际坐标并不相同，导致激光并未打在检查点上。因此在此置对每个点单独动态调整的偏移量：每个点的坐标
发送后会查看红色激光的位置，如果不相同就动态调整偏移量，直到横、纵坐标误差均不小于一个像素，再发送下一坐标。
'''
now_side = 0
now_point = 0
delta_x = 0
delta_y = 0
first_try = True

def Question_3():
    global now_side
    global now_point
    img = sensor.snapshot() # 实时拍照.

    ## 再进行绿色激光斑点查询
    #find_green_spot(img)
    # 再发送下一个检查点
    send_next_way(img)

    #img.draw_cross(tuple(spot_location[0:2]))
    #img.draw_cross(tuple(spot_location[2:4]))
    #img.draw_arrow(spot_location[2], spot_location[3], line_orientation[0], line_orientation[1])
    try:
        lcd.display(img)
    except:
        pass

Question_state = 1
moving_state = 0
moving_table = [
        [0, 0],
        [0, 0],
        [0, 120],
        [120, 120],
        [120, 0],
        [0, 0]
    ]
if_continue_send = True

find_total_way()

while True:
    tmp = uart_A.read()
    if tmp != None:
        print(tmp)
        if tmp.startswith(b'000') or tmp.endswith(b'000') or tmp.startswith(b'444') or tmp.endswith(b'444'):
            if_continue_send = True
        if tmp.startswith(b'111') or tmp.endswith(b'111') or Question_state == 1:
            if Question_state != 1:
                print("正在回答问题1")
            moving_state = 0

            now_side = 0
            now_point = 0

            Question_state = 1
            if_continue_send = True
            Question_1()

        if tmp == b'222'or tmp.startswith(b'222') or tmp.endswith(b'222') or Question_state == 2:
            if Question_state != 2:
                print("正在回答问题2")

            now_side = 0
            now_point = 0

            Question_state = 2
            if_continue_send = True
            Question_2()
        if tmp.startswith(b'333') or tmp.endswith(b'333') or Question_state == 3:
            if Question_state != 3:
                print("正在回答问题3")

            moving_state = 0

            Question_state = 3
            if_continue_send = True
            Question_3()
        if tmp.startswith(b'555') or tmp.endswith(b'555') or Question_state == 5:
            if Question_state != 5:
                print("调试模式，激光将指向(0, 0)")

            moving_state = 0

            now_side = 0
            now_point = 0

            Question_state = 5
            if_continue_send = True
            sending_data(0, 0, uart_A)
            sending_data(0, 0, uart_B)
    if_continue_send = False
